#include <omg/dds.hpp>
#include "radar_track_type.hpp"

/* Copyright (c) 2009-2010, Real-Time Innovations, Inc.
 * Copyright (c) 2010, Object Management Group, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 
 * - Redistributions of source code must retain the above copyright notice,
 *   this list of conditions and the following disclaimer.
 * - Redistributions in binary form must reproduce the above copyright
 *   notice, this list of conditions and the following disclaimer in the
 *   documentation and/or other materials provided with the distribution.
 * - Neither the names of the above copyright holders nor the names of their
 *   contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL A COPYRIGHT HOLDER OR CONTRIBUTOR BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */


static void pub_example()
{
    using dds::core::Duration;
    using dds::core::Exception;
    using dds::core::policy::Ownership;
    using dds::core::policy::Reliability;
    using dds::domain::DomainParticipant;
    using dds::domain::DomainParticipantFactory;
    using dds::topic::Topic;
    using dds::pub::DataWriter;
    using dds::pub::DataWriterQos;
    using dds::pub::Publisher;
    
    try {
        // --- Configuration --- //
        DomainParticipant dp =
            DomainParticipantFactory::get_instance().create_participant();
        Topic<RadarTrack> tp = dp.create_topic<RadarTrack>("Radar Tracks");
        Publisher pub = dp.create_publisher();
        DataWriterQos dwq = pub.default_data_writer_qos();
        // Set QoS policies like this:
        dwq.reliability().kind(Reliability::RELIABLE_KIND);
        dwq.ownership().kind(Ownership::EXCLUSIVE_KIND);
        // ...or like this:
        dwq << Reliability(Reliability::RELIABLE_KIND,
                           Duration::infinite())
            << Ownership(Ownership::EXCLUSIVE_KIND);
        DataWriter<RadarTrack> dw = pub.create_datawriter(tp, dwq);
        
        // --- Writing --- //
        RadarTrack sample;
        sample.track_id = 123;
        sample.latitude = 12.0;
        sample.longitude = 29.5;
        dw.write(sample);
        
        // --- Shutdown --- //
        dp.close();
    } catch (const Exception& ex) {
        // log exception
    }
}


static void sub_example()
{
    using std::vector;
    using dds::core::Duration;
    using dds::core::Exception;
    using dds::core::WaitSet;
    using dds::core::policy::Ownership;
    using dds::core::policy::Reliability;
    using dds::domain::DomainParticipant;
    using dds::domain::DomainParticipantFactory;
    using dds::topic::Topic;
    using dds::sub::DataReader;
    using dds::sub::DataReaderQos;
    using dds::sub::LoanedSamples;
    using dds::sub::Sample;
    using dds::sub::Subscriber;
    
    try {
        // --- Configuration --- //
        DomainParticipant dp =
            DomainParticipantFactory::get_instance().create_participant();
        Topic<RadarTrack> tp = dp.create_topic<RadarTrack>("Radar Tracks");
        Subscriber sub = dp.create_subscriber();
        DataReaderQos drq = sub.default_datareader_qos();
        // Set QoS policies like this:
        drq.reliability().kind(Reliability::RELIABLE_KIND);
        drq.ownership().kind(Ownership::EXCLUSIVE_KIND);
        // ...or like this:
        drq << Reliability(Reliability::RELIABLE_KIND,
                           Duration::infinite())
            << Ownership(Ownership::EXCLUSIVE_KIND);
        DataReader<RadarTrack> dr = sub.create_datareader(tp, drq);

        // --- Read --- //
        WaitSet ws = WaitSet::newWaitSet();
        ws.attach_condition(dr.create_readcondition());
        while (true) {
            ws.wait();
            // Take a loan out on multiple samples like this:
            LoanedSamples<RadarTrack> dt = dr.take();
            for (LoanedSamples<RadarTrack>::Iterator it  = dt.begin();
                 it != dt.end();
                 ++it) {
                const Sample<RadarTrack>& sample = *it;
                if (sample.is_valid_data()) {
                    const RadarTrack& data = sample.data();
                    // ...
                }
            }
            // ...or get a copy like this:
            vector< Sample<RadarTrack> > my_copy(dt.begin(), dt.end());
        }
        
        // --- Shutdown --- //
        dp.close();
    } catch (const Exception& ex) {
        // log exception
    }
}
